#include "cv.h"
#include "highgui.h"
#include <iostream>
#include <fstream>
#include <vector>
#include "types.h"

#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <sys/ioctl.h>
#include <stdlib.h>

#include <linux/types.h>
#include <linux/videodev.h>

#define VIDEO_DEV "/dev/video0"

#define READ_VIDEO_PIXEL(buf, format, depth, r, g, b)                   \
{                                                                       \
        switch (format)                                                 \
        {                                                               \
                case VIDEO_PALETTE_GREY:                                \
                        switch (depth)                                  \
                        {                                               \
                                case 4:                                 \
                                case 6:                                 \
                                case 8:                                 \
                                        (r) = (g) = (b) = (*buf++ << 8);\
                                        break;                          \
                                                                        \
                                case 16:                                \
                                        (r) = (g) = (b) =               \
                                                *((unsigned short *) buf);      \
                                        buf += 2;                       \
                                        break;                          \
                        }                                               \
                        break;                                          \
                                                                        \
                                                                        \
                case VIDEO_PALETTE_RGB565:                              \
                {                                                       \
                        unsigned short tmp = *(unsigned short *)buf;    \
                        (r) = tmp&0xF800;                               \
                        (g) = (tmp<<5)&0xFC00;                          \
                        (b) = (tmp<<11)&0xF800;                         \
                        buf += 2;                                       \
                }                                                       \
                break;                                                  \
                                                                        \
                case VIDEO_PALETTE_RGB555:                              \
                        (r) = (buf[0]&0xF8)<<8;                         \
                        (g) = ((buf[0] << 5 | buf[1] >> 3)&0xF8)<<8;    \
                        (b) = ((buf[1] << 2 ) & 0xF8)<<8;               \
                        buf += 2;                                       \
                        break;                                          \
                                                                        \
                case VIDEO_PALETTE_RGB24:                               \
                        (r) = buf[0] << 8; (g) = buf[1] << 8;           \
                        (b) = buf[2] << 8;                              \
                        buf += 3;                                       \
                        break;                                          \
                                                                        \
                default:                                                \
                        fprintf(stderr,                                 \
                                "Format %d not yet supported\n",        \
                                format);                                \
        }                                                               \
}

using namespace cv;

int get_brightness_adj(unsigned char *image, long size, int *brightness) {
    long i, tot = 0;
    for (i = 0; i < size * 3; i++) {
        tot += image[i];
    }
    *brightness = (128 - tot / (size * 3)) / 3;
    return !((tot / (size * 3)) >= 126 && (tot / (size * 3)) <= 130);
}

void Capture(cv::Mat *Frame) 
{
    SResult Result;
    int fd = open(VIDEO_DEV, O_RDONLY), f;
    struct video_capability cap;
    struct video_window win;
    struct video_picture vpic;

    unsigned char *buffer, *src;
    int bpp = 24, r = 0, g = 0, b = 0;
    unsigned int src_depth = 16;

    if (Result) {
        Result.SetResult
                (
                !(fd < 0)
                ) << "wrong video apparaat ";
    }

    if (Result) {
        Result.SetResult(
                !(ioctl(fd, VIDIOCGCAP, &cap) < 0)
                ) << "no v4l device";
    }

    if (Result) {
        Result.SetResult(
                !(ioctl(fd, VIDIOCGWIN, &win) < 0)
                ) << "no window";
    }

    if (Result) {
        Result.SetResult(
                !(ioctl(fd, VIDIOCGPICT, &vpic) < 0)
                ) << "no picture";
    }

    if (Result) {

        if (cap.type & VID_TYPE_MONOCHROME) {
            vpic.depth = 8;
            vpic.palette = VIDEO_PALETTE_GREY; /* 8bit grey */
            if (ioctl(fd, VIDIOCSPICT, &vpic) < 0) {
                vpic.depth = 6;
                if (ioctl(fd, VIDIOCSPICT, &vpic) < 0) {
                    vpic.depth = 4;
                    Result.SetResult(
                            !(ioctl(fd, VIDIOCSPICT, &vpic) < 0)
                            ) << "no cap format";
                }
            }
        } else {
            vpic.depth = 24;
            vpic.palette = VIDEO_PALETTE_RGB24;

            if (ioctl(fd, VIDIOCSPICT, &vpic) < 0) {
                vpic.palette = VIDEO_PALETTE_RGB565;
                vpic.depth = 16;

                if (ioctl(fd, VIDIOCSPICT, &vpic) == -1) {
                    vpic.palette = VIDEO_PALETTE_RGB555;
                    vpic.depth = 15;
                    Result.SetResult(
                            !(ioctl(fd, VIDIOCSPICT, &vpic) == -1)
                            ) << "Unable to find a supported capture format.";
                }
            }
        }
    }

    if (Result) {
        buffer = reinterpret_cast<unsigned char*> (malloc(win.width * win.height * bpp));
        Result.SetResult(buffer != 0) << "Out of memory.";
    }

    if (Result) {
        do {
            int newbright;
            read(fd, buffer, win.width * win.height * bpp);
            f = get_brightness_adj(buffer, win.width * win.height, &newbright);
            if (f) {
                vpic.brightness += (newbright << 8);
                Result.SetResult(!(ioctl(fd, VIDIOCSPICT, &vpic) == -1)) << "VIDIOSPICT";
            }
        } while (f);
    }

        fprintf(stdout, "P6\n%d %d 255\n", win.width, win.height);
		        fprintf(stdout, "pal %d \n",vpic.depth);

    src = buffer;

    for (int i = 0; i < win.height; i++) {
        for (int j = 0; j < win.width; j++) {
            READ_VIDEO_PIXEL(src, vpic.palette, src_depth, r, g, b);
            //Frame->at<unsigned char>(i, j+0) = (unsigned char) (r>>8);
            Frame->at<unsigned char>(i, j) = (unsigned char) (g>>8);
            //Frame->at<unsigned char>(i, j+2) = (unsigned char) (b>>8);

            //fputc(b >> 8, stdout);
            //fputc(g >> 8, stdout);
            //fputc(r >> 8, stdout);
	        //fprintf(stdout, "Row %d Col %d \n",i,j);
        }
	
    }

    close(fd);
}
		
int main(int, char**)
{

    //cv::Mat Frame(win.height, win.width, CV_8UC3);
     

//   
	SResult Result;
    int fd = open(VIDEO_DEV, O_RDONLY), f;
    struct video_capability cap;
    struct video_window win;
    struct video_picture vpic;

    unsigned char *buffer, *src;
    int bpp = 24, r = 0, g = 0, b = 0;
    unsigned int src_depth = 16;

    if (Result) {
        Result.SetResult
                (
                !(fd < 0)
                ) << "wrong video apparaat ";
    }

    if (Result) {
        Result.SetResult(
                !(ioctl(fd, VIDIOCGCAP, &cap) < 0)
                ) << "no v4l device";
    }

    if (Result) {
        Result.SetResult(
                !(ioctl(fd, VIDIOCGWIN, &win) < 0)
                ) << "no window";
    }

    if (Result) {
        Result.SetResult(
                !(ioctl(fd, VIDIOCGPICT, &vpic) < 0)
                ) << "no picture";
    }

    if (Result) {

        if (cap.type & VID_TYPE_MONOCHROME) {
            vpic.depth = 8;
            vpic.palette = VIDEO_PALETTE_GREY; /* 8bit grey */
            if (ioctl(fd, VIDIOCSPICT, &vpic) < 0) {
                vpic.depth = 6;
                if (ioctl(fd, VIDIOCSPICT, &vpic) < 0) {
                    vpic.depth = 4;
                    Result.SetResult(
                            !(ioctl(fd, VIDIOCSPICT, &vpic) < 0)
                            ) << "no cap format";
                }
            }
        } else {
            vpic.depth = 24;
            vpic.palette = VIDEO_PALETTE_RGB24;

            if (ioctl(fd, VIDIOCSPICT, &vpic) < 0) {
                vpic.palette = VIDEO_PALETTE_RGB565;
                vpic.depth = 16;

                if (ioctl(fd, VIDIOCSPICT, &vpic) == -1) {
                    vpic.palette = VIDEO_PALETTE_RGB555;
                    vpic.depth = 15;
                    Result.SetResult(
                            !(ioctl(fd, VIDIOCSPICT, &vpic) == -1)
                            ) << "Unable to find a supported capture format.";
                }
            }
        }
    }

    if (Result) {
        buffer = reinterpret_cast<unsigned char*> (malloc(win.width * win.height * bpp));
        Result.SetResult(buffer != 0) << "Out of memory.";

    }
  for(;;)
    {
 /*   if (Result) {
        do {
            int newbright;
            read(fd, buffer, win.width * win.height * bpp);
            f = get_brightness_adj(buffer, win.width * win.height, &newbright);
            if (f) {
                vpic.brightness += (newbright << 8);
                Result.SetResult(!(ioctl(fd, VIDIOCSPICT, &vpic) == -1)) << "VIDIOSPICT";
            }
        } while (f);
    }
*/
	read(fd, buffer, win.width * win.height * bpp);
	cv::Mat Frame(480,640,CV_8UC3, buffer);

        fprintf(stdout, "P6\n%d %d 255\n", win.width, win.height);
		        fprintf(stdout, "pal %d \n",vpic.depth);

    src = buffer;
    namedWindow("edges",1);

/*
    for (int i = 0; i < win.height; i++) {
        for (int j = 0; j < win.width; j++) {

            READ_VIDEO_PIXEL(src, vpic.palette, src_depth, r, g, b);
            Frame.at<int>(i, j) = (int)((r >> 8 ) << 16) & ((g >> 8) << 8) & ( b>>8 ) ;
           // Frame.at<unsigned char>(i, j) = (unsigned char) (g>>8);
           // Frame.at<unsigned char>(i, j) = (unsigned char) (b>>8);

            //fputc(b >> 8, stdout);
            //fputc(g >> 8, stdout);
            //fputc(r >> 8, stdout);
	        //fprintf(stdout, "Row %d Col %d \n",i,j);
        }
	
    }
*/

       imshow("edges", Frame);
        if(waitKey(30) >= 0) break;
    }
    close(fd);
    // the camera will be deinitialized automatically in VideoCapture destructor
    return 0;
}
